#include <iostream>
#include "jizhi_gnis.h"
#include "mount_haofu.h"
#include <thread>
#include <math.h>


#ifdef PLATFORM_ARCH_x86_64
#define JIZHI_UART					"/dev/ttyUSB0"
#define JIZHI_UART_BAUDRATE   		"115200"
#else
//firefly
//#define JIZHI_UART					"/dev/ttyACM0"
//orangepi
#define JIZHI_UART					"/dev/ttyUSB0"
#define JIZHI_UART_BAUDRATE   		"115200"
#endif

using namespace std;

void haofuUpdateHandler(const char *ip);

int ttyConfigure(char *ttyPort, char *baudrate);

static void sleepMs(uint32_t timeMs)
{
	usleep(timeMs * 1000);
}

int getBit(unsigned char byte, int position)
{
    return (byte >> position) & 1;
}


double wrap360(const double angle)
{
    double res = fmod(angle, 360.0);
    if (res < 0)
    {
        res += 360.0;
    }
    return res;
}


double wrap180(const double angle)
{
    auto res = wrap360(angle);
    if (res > 180)
	{
        res -= 360;
    }
    return res;
}


int main(int argc, char **argv)
{
	cout << "jizhi data to haofu."<< endl;

	if(argc < 2)
	{
		printf("Need to input IP address!!!\n");
		return -1;
	}

	printf("haofu ip:%s\n", argv[1]);

	int ttyFd;
	ttyFd = ttyConfigure(JIZHI_UART, JIZHI_UART_BAUDRATE);

	if(ttyFd < 0)
	{
		printf("ttyConfigure failed\n");
		return -1;
	}

	cout << "start haofu update handler...." << endl;
	//start haofu update
	std::thread haofuUpdate(haofuUpdateHandler, argv[1]);


	uint8_t buf[512];
	GnisProtocolType msg;

	while(1)
	{
		sleepMs(2);
		int len;
		do{
			len = 0;
			bzero(buf, sizeof(buf));
			len = read(ttyFd, buf, sizeof(buf));

			if(len < 0)
			{
				printf("jizhi Recv error\n");
			}

			for(int i=0; i<len; i++)
			{
				if(ProtocolParseChar(&msg, buf[i]))
				{
					float pitch = 0.0f;
					float roll = 0.0f;
					float yaw = 0.0f;
					int32_t lat = 0;
					int32_t lng = 0;
					int32_t alt = 0;

					lat = msg.m_lat;
					lng = msg.m_lng;
					alt = msg.m_alt * 1000;

					pitch = msg.m_pitch * (360.0f/65536.0f);
					roll = msg.m_roll * (360.0f/65536.0f);
					yaw = wrap360(msg.m_yaw * (360.0f/65536.0f));

					//printf("lat:%d, lng:%d, lat:%d, pitch:%f, roll:%f, yaw:%f\n", lat, lng, alt, pitch, roll, yaw);

					mountHaofuHandler.setCopterGpsToHaofu(pitch, roll, wrap180(yaw), lat, lng, alt, 50, 50);
				}
			}
		}while(len == sizeof(buf));
	}

	return 0;
}


void haofuUpdateHandler(const char *ip)
{
	printf("ip:%s\n", ip);
	bool ret = mountHaofuHandler.init(ip);
	if(!ret)
	{
		printf("haofu init error\n");
		return;
	}

	mountHaofuHandler.update();
}


int ttyConfigure(char *ttyPort, char *baudrate)
{
	int ttyFd;
	struct termios  oldTio = {0};
	struct termios newCfg = {0};
	int rate;

	if(ttyPort == NULL || baudrate == NULL)
	{
		return -1;
	}

	ttyFd = open(ttyPort, O_RDWR | O_NOCTTY );

	if(ttyFd < 0 )
	{
		cout << "open " << ttyPort << "error" << endl;
		return -1;
	}

	if(tcgetattr(ttyFd, &oldTio) != 0)
	{
		cout << "tcgettattr failed" << endl;
		close(ttyFd);
		return -1;
	}

	if(strcmp(baudrate, "38400") == 0)
	{
		rate = B38400;
	}
	else if(strcmp(baudrate, "57600") == 0)
	{
		rate = B57600;
	}
	else if(strcmp(baudrate, "115200") == 0)
	{
		rate = B115200;
	}
	else if(strcmp(baudrate, "230400") == 0)
	{
		rate = B230400;
	}
	else
	{
		cout << "baud rate:" << baudrate << "is not support!" << endl;
		close(ttyFd);
		return -1;
	}

	//set the raw model
	cfmakeraw(&newCfg);
	newCfg.c_cflag |= CREAD;

	if(cfsetspeed(&newCfg, rate) < 0)
	{
		cout << "cfsetispeed error:" << baudrate << endl;
	}

	newCfg.c_cflag &= ~CSIZE;
	newCfg.c_cflag |= CS8;
	newCfg.c_cflag &= ~PARENB;
	newCfg.c_iflag &= ~INPCK;
	newCfg.c_cflag &= ~CSTOPB;
	newCfg.c_cc[VTIME] = 0;
	newCfg.c_cc[VMIN] = 0;

	if (tcflush(ttyFd, TCIOFLUSH) < 0)
	{
		cout << "tcflush error" << endl;
		return -1;
	}

	if(tcsetattr(ttyFd, TCSANOW, &newCfg) < 0)
	{
		cout << "tcsetattr failed" << endl;
		close(ttyFd);
		return -1;
	}

	return ttyFd;
}
